# -- coding: utf-8 --

import queue
import sys
import threading
import msvcrt
import numpy as np
from ctypes import *

import math
import cv2
import time
import inspect
# sys.path.append("../MvImport")
from MvCameraControl_class import *


def Async_raise(tid, exctype):
    tid = ctypes.c_long(tid)
    if not inspect.isclass(exctype):
        exctype = type(exctype)
    res = ctypes.pythonapi.PyThreadState_SetAsyncExc(
        tid, ctypes.py_object(exctype))
    if res == 0:
        raise ValueError("invalid thread id")
    elif res != 1:
        ctypes.pythonapi.PyThreadState_SetAsyncExc(tid, None)
        raise SystemError("PyThreadState_SetAsyncExc failed")


def Stop_thread(thread):
    Async_raise(thread.ident, SystemExit)


def Color_numpy(data, nWidth, nHeight):
    data_ = np.frombuffer(data, count=int(
        nWidth*nHeight*3), dtype=np.uint8, offset=0)
    data_r = data_[0:nWidth*nHeight*3:3]
    data_g = data_[1:nWidth*nHeight*3:3]
    data_b = data_[2:nWidth*nHeight*3:3]

    data_r_arr = data_r.reshape(nHeight, nWidth)
    data_g_arr = data_g.reshape(nHeight, nWidth)
    data_b_arr = data_b.reshape(nHeight, nWidth)
    numArray = np.zeros([nHeight, nWidth, 3], "uint8")

    numArray[:, :, 0] = data_r_arr
    numArray[:, :, 1] = data_g_arr
    numArray[:, :, 2] = data_b_arr
    return numArray


def Work_thread(self):
    stOutFrame = MV_FRAME_OUT()
    img_buff = None
    buf_cache = None
    numArray = None
    self.b_save_jpg = False
    self.b_save_bmp = False
    while True:
        ret = self.cam.MV_CC_GetImageBuffer(stOutFrame, 1000)
        print('MV_CC_GetImageBuffer', ret)
        if 0 == ret:
            if None == buf_cache:
                buf_cache = (c_ubyte * stOutFrame.stFrameInfo.nFrameLen)()
            # 获取到图像的时间开始节点获取到图像的时间开始节点
            self.st_frame_info = stOutFrame.stFrameInfo
            cdll.msvcrt.memcpy(
                byref(buf_cache), stOutFrame.pBufAddr, self.st_frame_info.nFrameLen)
            print("get one frame: Width[%d], Height[%d], nFrameNum[%d]" % (
                self.st_frame_info.nWidth, self.st_frame_info.nHeight, self.st_frame_info.nFrameNum))
            self.n_save_image_size = self.st_frame_info.nWidth * \
                self.st_frame_info.nHeight * 3 + 2048
            if img_buff is None:
                img_buff = (c_ubyte * self.n_save_image_size)()

            if True == self.b_save_jpg:
                self.Save_jpg(buf_cache)  # ch:保存Jpg图片 | en:Save Jpg
            if True == self.b_save_bmp:
                self.Save_Bmp(buf_cache)  # ch:保存Bmp图片 | en:Save Bmp
        else:
            print("no data, nret = ", (ret))
            continue

        # 转换像素结构体赋值
        stConvertParam = MV_CC_PIXEL_CONVERT_PARAM()
        memset(byref(stConvertParam), 0, sizeof(stConvertParam))
        stConvertParam.nWidth = self.st_frame_info.nWidth
        stConvertParam.nHeight = self.st_frame_info.nHeight
        stConvertParam.pSrcData = cast(buf_cache, POINTER(c_ubyte))
        stConvertParam.nSrcDataLen = self.st_frame_info.nFrameLen
        stConvertParam.enSrcPixelType = self.st_frame_info.enPixelType

        # RGB直接显示
        if PixelType_Gvsp_RGB8_Packed == self.st_frame_info.enPixelType:
            numArray = Color_numpy(
                buf_cache, self.st_frame_info.nWidth, self.st_frame_info.nHeight)

        # 如果是彩色且非RGB则转为RGB后显示
        else:
            nConvertSize = self.st_frame_info.nWidth * self.st_frame_info.nHeight * 3
            stConvertParam.enDstPixelType = PixelType_Gvsp_RGB8_Packed
            stConvertParam.pDstBuffer = (c_ubyte * nConvertSize)()
            stConvertParam.nDstBufferSize = nConvertSize
            time_start = time.time()
            ret = self.cam.MV_CC_ConvertPixelType(stConvertParam)
            time_end = time.time()
            print('MV_CC_ConvertPixelType:', time_end - time_start)
            if ret != 0:
                print('convert pixel fail! ret = ', (ret))
                continue
            cdll.msvcrt.memcpy(
                byref(img_buff), stConvertParam.pDstBuffer, nConvertSize)
            numArray = Color_numpy(
                img_buff, self.st_frame_info.nWidth, self.st_frame_info.nHeight)

        # 合并OpenCV到Tkinter界面中
        if self.frame_callback is not None:
            self.frame_callback(numArray)

        nRet = self.cam.MV_CC_FreeImageBuffer(stOutFrame)
        if self.b_exit == True:
            if img_buff is not None:
                del img_buff
            if buf_cache is not None:
                del buf_cache
            break


class MVSCam:
    deviceList = MV_CC_DEVICE_INFO_LIST()
    tlayerType = MV_GIGE_DEVICE | MV_USB_DEVICE

    # ch:枚举设备 | en:Enum device
    ret = MvCamera.MV_CC_EnumDevices(tlayerType, deviceList)
    if ret != 0:
        print("enum devices fail! ret[0x%x]" % ret)
        sys.exit()

    if deviceList.nDeviceNum == 0:
        print("find no device!")
        sys.exit()

    print("Find %d devices!" % deviceList.nDeviceNum)

    for i in range(0, deviceList.nDeviceNum):
        mvcc_dev_info = cast(deviceList.pDeviceInfo[i], POINTER(
            MV_CC_DEVICE_INFO)).contents
        if mvcc_dev_info.nTLayerType == MV_GIGE_DEVICE:
            print("\ngige device: [%d]" % i)
            strModeName = ""
            for per in mvcc_dev_info.SpecialInfo.stGigEInfo.chModelName:
                strModeName = strModeName + chr(per)
            print("device model name: %s" % strModeName)

            nip1 = (
                (mvcc_dev_info.SpecialInfo.stGigEInfo.nCurrentIp & 0xff000000) >> 24)
            nip2 = (
                (mvcc_dev_info.SpecialInfo.stGigEInfo.nCurrentIp & 0x00ff0000) >> 16)
            nip3 = (
                (mvcc_dev_info.SpecialInfo.stGigEInfo.nCurrentIp & 0x0000ff00) >> 8)
            nip4 = (mvcc_dev_info.SpecialInfo.stGigEInfo.nCurrentIp & 0x000000ff)
            print("current ip: %d.%d.%d.%d\n" % (nip1, nip2, nip3, nip4))
        elif mvcc_dev_info.nTLayerType == MV_USB_DEVICE:
            print("\nu3v device: [%d]" % i)
            strModeName = ""
            for per in mvcc_dev_info.SpecialInfo.stUsb3VInfo.chModelName:
                if per == 0:
                    break
                strModeName = strModeName + chr(per)
            print("device model name: %s" % strModeName)

            strSerialNumber = ""
            for per in mvcc_dev_info.SpecialInfo.stUsb3VInfo.chSerialNumber:
                if per == 0:
                    break
                strSerialNumber = strSerialNumber + chr(per)
            print("user serial number: %s" % strSerialNumber)

    def __init__(self):
        self.b_thread_closed = False
        self.b_start_grabbing = False

    def open(self, nConnectionNum, triggermode='off', frame_callback=None):
        #nConnectionNum = input("please input the number of the device to connect:")
        nConnectionNum = 0
        self.triggermode = triggermode
        self.frame_callback = frame_callback
        if self.b_start_grabbing:
            return 1

        if int(nConnectionNum) >= self.deviceList.nDeviceNum:
            print("intput error!")
            sys.exit()

        # ch:创建相机实例 | en:Creat Camera Object
        cam = MvCamera()
        self.cam = cam
        # ch:选择设备并创建句柄 | en:Select device and create handle
        stDeviceList = cast(self.deviceList.pDeviceInfo[int(
            nConnectionNum)], POINTER(MV_CC_DEVICE_INFO)).contents

        ret = cam.MV_CC_CreateHandle(stDeviceList)
        if ret != 0:
            print("create handle fail! ret[0x%x]" % ret)
            sys.exit()

        # ch:打开设备 | en:Open device
        ret = cam.MV_CC_OpenDevice(MV_ACCESS_Exclusive, 0)
        if ret != 0:
            print("open device fail! ret[0x%x]" % ret)
            sys.exit()

        # ch:探测网络最佳包大小(只对GigE相机有效) | en:Detection network optimal package size(It only works for the GigE camera)
        if stDeviceList.nTLayerType == MV_GIGE_DEVICE:
            nPacketSize = cam.MV_CC_GetOptimalPacketSize()
            if int(nPacketSize) > 0:
                ret = cam.MV_CC_SetIntValue("GevSCPSPacketSize", nPacketSize)
                if ret != 0:
                    print("Warning: Set Packet Size fail! ret[0x%x]" % ret)
            else:
                print("Warning: Get Packet Size fail! ret[0x%x]" % nPacketSize)

        if self.triggermode == 'off':
            # ch:设置触发模式为off | en:Set trigger mode as off
            ret = cam.MV_CC_SetEnumValue("TriggerMode", MV_TRIGGER_MODE_OFF)
            if ret != 0:
                print("set trigger mode fail! ret[0x%x]" % ret)
                sys.exit()
        else:
            # ch:设置触发模式为off | en:Set trigger mode as off
            ret = cam.MV_CC_SetEnumValue("TriggerMode", MV_TRIGGER_MODE_ON)
            if ret != 0:
                print("set trigger mode fail! ret[0x%x]" % ret)
                sys.exit()

            if self.triggermode == 'soft':
                ret = self.cam.MV_CC_SetEnumValue(
                    "TriggerSource", MV_TRIGGER_SOURCE_SOFTWARE)
                if ret != 0:
                    print('set triggersource fail! ret = ', (ret))
            else:
                ret = cam.MV_CC_SetEnumValue(
                    "TriggerSource", MV_TRIGGER_SOURCE_LINE0)
                if ret != 0:
                    print('set triggersource fail! ret = ', (ret))

        # ch:获取数据包大小 | en:Get payload size
        stParam = MVCC_INTVALUE()
        memset(byref(stParam), 0, sizeof(MVCC_INTVALUE))

        ret = cam.MV_CC_GetIntValue("PayloadSize", stParam)
        if ret != 0:
            print("get payload size fail! ret[0x%x]" % ret)
            sys.exit()
        nPayloadSize = stParam.nCurValue

        ret = cam.MV_CC_StartGrabbing()
        if ret != 0:
            print('show error', 'start grabbing fail! ret = ', (ret))
            return
        self.b_exit = False
        self.b_start_grabbing = True
        print("start grabbing successfully!")
        if not self.triggermode == 'off':
            try:
                #self.n_win_gui_id = random.randint(1,10000)
                self.h_thread_handle = threading.Thread(
                    target=Work_thread, args=(self,))
                self.h_thread_handle.start()
                self.b_thread_closed = True
            except:
                print('show error', 'error: unable to start thread')
                False == self.b_start_grabbing

        data_buf = (c_ubyte * nPayloadSize)()

        self.pData = byref(data_buf)
        self.nDataSize = nPayloadSize
        stFrameInfo = MV_FRAME_OUT_INFO_EX()
        memset(byref(stFrameInfo), 0, sizeof(stFrameInfo))
        self.cam = cam
        self.stFrameInfo = stFrameInfo
        self.data_buf = data_buf

    def getFrame(self):
        cam = self.cam
        pData = self.pData
        nDataSize = self.nDataSize
        stFrameInfo = self.stFrameInfo
        ret = cam.MV_CC_GetOneFrameTimeout(pData, nDataSize, stFrameInfo, 500)

        if ret == 0:
            #print ("get one frame: Width[%d], Height[%d], nFrameNum[%d]"  % (stFrameInfo.nWidth, stFrameInfo.nHeight, stFrameInfo.nFrameNum))
            pass
        else:
            print("no data[0x%x]" % ret)
            return None

        cn = nDataSize/(stFrameInfo.nHeight*stFrameInfo.nWidth)
        cn = int(math.ceil(cn))
        npArray = np.zeros(
            (stFrameInfo.nHeight, stFrameInfo.nWidth, cn), dtype=np.uint8)
        npArrayPtr = npArray.ctypes.data_as(POINTER(c_ubyte))
        memmove(npArrayPtr, pData, nDataSize)
        if PixelType_Gvsp_YUV422_YUYV_Packed == stFrameInfo.enPixelType:
            # print(cv2.COLOR_YUV2BGR_YUYV)
            # print(cv2.COLOR_YUV2BGR_Y422)
            npArray = cv2.cvtColor(npArray, cv2.COLOR_YUV2BGR_YUYV)

        return npArray

    def stop(self):
        cam = self.cam
        data_buf = self.data_buf
        if True == self.b_thread_closed:
            Stop_thread(self.h_thread_handle)
            self.b_thread_closed = False
        # msvcrt.getch()
        # ch:停止取流 | en:Stop grab image
        ret = cam.MV_CC_StopGrabbing()
        if ret != 0:
            print("stop grabbing fail! ret[0x%x]" % ret)
            del data_buf
            sys.exit()

        # ch:关闭设备 | Close device
        ret = cam.MV_CC_CloseDevice()
        if ret != 0:
            print("close deivce fail! ret[0x%x]" % ret)
            del data_buf
            sys.exit()

        # ch:销毁句柄 | Destroy handle
        ret = cam.MV_CC_DestroyHandle()
        if ret != 0:
            print("destroy handle fail! ret[0x%x]" % ret)
            del data_buf
            sys.exit()

        del data_buf


def test_MVSCam():
    cam = MVSCam()
    cam.open(0)
    for i in range(100):
        im = cam.getFrame()
        im = cv2.resize(im, (640, 480))
        cv2.imshow('im', im)
        print(i)
        cv2.waitKey(100)

    cam.stop()


def test_MVSCam_tri():
    cam = MVSCam()
    import queue
    q = queue.Queue(maxsize=1)
    def frame_callback(im):
        im = cv2.resize(im, (640, 480))
        q.put(im)

    cam.open(0, 'hard', frame_callback)
    im = np.zeros((480, 640, 3), dtype='uint8')
    for i in range(1000):
        try:
            im = q.get(False, 0.1)
        except queue.Empty:
            pass
        cv2.imshow('im', im)
        cv2.waitKey(100)
        print('time out')

    cam.stop()


if __name__ == "__main__":
    test_MVSCam_tri()
